
/****************************************************************************
*						DataWriter Class implementation						*
*    				Copyright (C) 2012  J.R. Ruiz-Sarmiento					*
*																			*
*    This program is free software: you can redistribute it and/or modify	*
*    it under the terms of the GNU General Public License as published by	*
*    the Free Software Foundation, either version 3 of the License, or		*
*    (at your option) any later version.									*
*																			*
*    This program is distributed in the hope that it will be useful,		*
*    but WITHOUT ANY WARRANTY; without even the implied warranty of			*
*    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the			*
*    GNU General Public License for more details.							*
*																			*
*    You should have received a copy of the GNU General Public License		*
*    along with this program.  If not, see <http://www.gnu.org/licenses/>.	*
*																			*
****************************************************************************/


#include "CDataWriter.h"

using namespace SORT::DataIO;
using namespace SORT::ObjectRecognition;

void CPlanesWriter::writeToXMLFile( const string &fileName, mrpt::aligned_containers<CPlane>::vector_t &v_planes )
{
	TiXmlDocument doc;  // Document to write
	TiXmlComment *comment;

	size_t N_planes = v_planes.size();
	
	TiXmlDeclaration* decl = new TiXmlDeclaration( "1.0", "", "" );  
	doc.LinkEndChild( decl ); 
 
	TiXmlElement * root = new TiXmlElement("PlanesList");  
	root->SetAttribute("version", "0.1");
	root->SetAttribute("size", N_planes );
	doc.LinkEndChild( root );  

	for ( size_t i = 0; i < N_planes; i++ )
	{
		TiXmlElement * plane = new TiXmlElement("Plane");  

		// Store the plane id
		plane->SetAttribute("id", v_planes[i].getId() );

		// Store the orientation
		string type;
		if ( v_planes[i].m_type == CPlane::TPlaneType::VERTICAL )
			type = "VERTICAL";
		else if (v_planes[i].m_type == CPlane::TPlaneType::ALMOST_VERTICAL )
			type = "ALMOST_VERTICAL";
		else if ( v_planes[i].m_type == CPlane::TPlaneType::HORIZONTAL )
			type = "HORIZONTAL";
		else if ( v_planes[i].m_type == CPlane::TPlaneType::ALMOST_HORIZONTAL )
			type = "ALMOST_HORIZONTAL";
		else if ( v_planes[i].m_type == CPlane::TPlaneType::DIAGONAL )
			type = "DIAGONAL";

		plane->SetAttribute("orientation", type.c_str() );

		// Store coordinates in the MRPT coordinate' system
		//                 
		//			   z ^	   ^
		//				 |    / x
		//				 |   /
		//				 |  /
		//				 | /
		//   y <---------|/
		//		
		plane->SetDoubleAttribute("centroid_x", v_planes[i].m_centroid[0]); // has_centroid_x
		plane->SetDoubleAttribute("centroid_y", v_planes[i].m_centroid[1]); // has_centroid_y
		plane->SetDoubleAttribute("centroid_z", v_planes[i].m_centroid[2]); // has_centroid_z

		plane->SetDoubleAttribute("area", v_planes[i].getHeight() * v_planes[i].getWidth() );

		plane->SetDoubleAttribute("elongation", v_planes[i].getElongation() );

		root->LinkEndChild( plane );
	}

	doc.SaveFile( fileName.c_str() );
}